#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/SceneObject.h>

#include "ModelPoseSensorVisualisation.h"

using namespace MMM;

ModelPoseSensorVisualisation::ModelPoseSensorVisualisation(ModelPoseSensorPtr sensor, VirtualRobot::RobotPtr robot, VirtualRobot::CoinVisualizationPtr visualization, SoSeparator* sceneSep) :
    SensorVisualisation(sceneSep),
    sensor(sensor),
    robot(robot)
{
    SoSeparator* robotSep = new SoSeparator();

    if (visualization) {
        SoNode* visualisationNode = visualization->getCoinVisualization();
        if (visualisationNode) robotSep->addChild(visualisationNode);
    }

    setVisualisationSep(robotSep);
}

std::string ModelPoseSensorVisualisation::getType() {
    return sensor->getType();
}

int ModelPoseSensorVisualisation::getPriority() {
    return sensor->getPriority();
}

void ModelPoseSensorVisualisation::update(float timestep, float delta) {
    update(sensor->getDerivedMeasurement(timestep, delta));
}

void ModelPoseSensorVisualisation::update(float timestep) {
    update(sensor->getDerivedMeasurement(timestep));
}

void ModelPoseSensorVisualisation::update(ModelPoseSensorMeasurementPtr measurement) {
    if (measurement) robot->setGlobalPose(measurement->getRootPose());
}
